package com.crunch.core;

// static utility functions
public class Util {
	// returns the result of dividing rounding down; assumes positive divisor
	public static int divideDownFix(int undividedValue, int divisor, int dividedValue) {
		if (undividedValue >= 0 || dividedValue*divisor == undividedValue) {
			return dividedValue;
		} else {
			return dividedValue - 1;
		}
	}

	// returns the result of dividing rounding up; assumes positive divisor
	public static int divideUpFix(int undividedValue, int divisor, int dividedValue) {
		if (undividedValue <= 0 || dividedValue*divisor == undividedValue) {
			return dividedValue;
		} else {
			return dividedValue + 1;
		}
	}

	// 3x3 matrix multiplication
	public static void matrixMul33(float[] matA, float[] matB, float[] out) {
		for (int y = 0; y < 3; ++y) {
			for (int x = 0; x < 3; ++x) {
				int idx = y*3+x;
				out[idx] = 0.0f;
				for (int i = 0; i < 3; ++i) {
					out[idx] += matA[y*3+i] * matB[i*3+x];
				}
			}
		}
	}

	// 4x4 matrix multiplication
	public static void matrixMul44(float[] matA, float[] matB, float[] out) {
		for (int y = 0; y < 4; ++y) {
			for (int x = 0; x < 4; ++x) {
				int idx = y*4+x;
				out[idx] = 0.0f;
				for (int i = 0; i < 4; ++i) {
					out[idx] += matA[y*4+i] * matB[i*4+x];
				}
			}
		}
	}

	public static void matrix33Vector3Mul(float[] mat, float[] vec, float[] out) {
		for (int y = 0; y < 3; ++y) {
			out[y] = 0.0f;
			for (int i = 0; i < 3; ++i) {
				out[y] += mat[y*3+i] * vec[i];
			}
		}
	}

	public static void matrix44Vector4Mul(float[] mat, float[] vec, float[] out) {
		for (int y = 0; y < 4; ++y) {
			out[y] = 0.0f;
			for (int i = 0; i < 4; ++i) {
				out[y] += mat[y*4+i] * vec[i];
			}
		}
	}

	public static float triangleArea(float ax, float ay, float bx, float by, float cx, float cy) {
		// heron's formula
		float abx = bx - ax;
		float aby = by - ay;
		float bcx = cx - bx;
		float bcy = cy - by;
		float cax = ax - cx;
		float cay = ay - cy;
		float ab = (float) Math.sqrt(abx*abx + aby*aby);
		float bc = (float) Math.sqrt(bcx*bcx + bcy*bcy);
		float ca = (float) Math.sqrt(cax*cax + cay*cay);
		float s = 0.5f * (ab + bc + ca);
		return (float) Math.sqrt(s*(s - ab)*(s - bc)*(s - ca));
	}

	public static boolean isPointInTriangle(float px, float py, float ax, float ay, float bx, float by, float cx, float cy) {
		float abx = bx - ax;
		float aby = by - ay;
		float acx = cx - ax;
		float acy = cy - ay;

		float dotBB = Util.mag2(abx, aby);
		float dotBC = Util.dot(abx, aby, acx, acy);
		float dotCC = Util.mag2(acx, acy);
		float invDenom = 1.0f / (dotBB*dotCC-dotBC*dotBC);

		float apx = px - ax;
		float apy = py - ay;
		float dotBP = Util.dot(abx, aby, apx, apy);
		float dotCP = Util.dot(acx, acy, apx, apy);
		float u = (dotCC*dotBP - dotBC*dotCP)*invDenom;
		if (u < 0.0f) {
			return false;
		}
		float v = (dotBB*dotCP - dotBC*dotBP)*invDenom;
		return (v >= 0.0f && u + v <= 1.0f);
	}

	public static float dot(float ax, float ay, float bx, float by) {
		return ax*bx + ay*by;
	}

	public static float mag2(float x, float y) {
		return x*x + y*y;
	}

	public static float dist2(float ax, float ay, float bx, float by) {
		float x = bx - ax;
		float y = by - ay;
		return mag2(x, y);
	}

	public static boolean invertMat2(float[] in, float[] out) {
		float det = in[0]*in[3] - in[2]*in[1];
		if (det == 0.0f) {
			return false;
		}
		float invDet = 1.0f / det;
		out[0] = invDet * in[3];
		out[1] = invDet * -in[1];
		out[2] = invDet * -in[2];
		out[3] = invDet * in[0];
		return true;
	}

	public static float dist2PointLine(float px, float py, float ax, float ay, float bx, float by) {
		// project point onto line to find closest point
		float abx = bx - ax;
		float aby = by - ay;
		float apx = px - ax;
		float apy = py - ay;
		float proj = dot(abx, aby, apx, apy) / mag2(abx, aby);
		float cx = proj*abx;
		float cy = proj*aby;
		return dist2(apx, apy, cx, cy);
	}

	public static float dist2PointLineSegment(float px, float py, float ax, float ay, float bx, float by) {
		// project point onto line to find closest point
		float abx = bx - ax;
		float aby = by - ay;
		float apx = px - ax;
		float apy = py - ay;
		float proj = dot(abx, aby, apx, apy) / mag2(abx, aby);
		proj = Math.max(0.0f, Math.min(1.0f, proj));
		float cx = proj*abx;
		float cy = proj*aby;
		return dist2(apx, apy, cx, cy);
	}

	public static float[] projectPointOntoLine(float x, float y, float ax, float ay, float bx, float by) {
		float vx = bx - ax;
		float vy = by - ay;
		float proj = Util.dot(x - ax, y - ay, vx, vy) / Util.mag2(vx, vy);
		return new float[] {
				ax + proj*vx,
				ay + proj*vy
		};
	}

	public static float pointLineProjectionRatio(float x, float y, float ax, float ay, float bx, float by) {
		float vx = bx - ax;
		float vy = by - ay;
		return Util.dot(x - ax, y - ay, vx, vy) / Util.mag2(vx, vy);
	}

	// normal is determined by rotating vector (b-a) CW
	public static float side(float x, float y, float ax, float ay, float bx, float by) {
		float nx = by - ay; // y
		float ny = ax - bx; // -x
		float px = x - ax;
		float py = y - ay;
		return Util.dot(px, py, nx, ny);
	}

	public static boolean doLinesIntersect(float ax1, float ay1, float bx1, float by1, float ax2, float ay2, float bx2, float by2) {
		boolean aCrossesB = (Math.signum(side(ax1, ay1, ax2, ay2, bx2, by2)) != Math.signum(side(bx1, by1, ax2, ay2, bx2, by2)));
		boolean bCrossesA = (Math.signum(side(ax2, ay2, ax1, ay1, bx1, by1)) != Math.signum(side(bx2, by2, ax1, ay1, bx1, by1)));
		return aCrossesB && bCrossesA;
	}

	public static boolean isCcw(float ax, float ay, float bx, float by, float cx, float cy) {
		float abx = bx-ax;
		float aby = by-ay;
		float acx = cx-ax;
		float acy = cy-ay;
		// check z component of ab x ac
		return (abx*acy - acx*aby > 0.0f);
	}

	public static float[] intersectLines(float ax1, float ay1, float bx1, float by1, float ax2, float ay2, float bx2, float by2) {
		// solve for ax + by = c for line 1
		float a = by1 - ay1; // y
		float b = bx1 - ax1; // -x
		float c = Util.dot(ax1, ay1, a, b);

		// find distance of points a and b - solve for when distance is 0
		float distA = a*ax2 + b*ay2 - c;
		float distB = a*bx2 + b*by2 - c;
		float denom = distB - distA;
		if (denom == 0.0f) {
			return null;
		}
		float ratio = -distA / denom;
		return new float[] {
				ax2 + ratio*(bx2-ax2),
				ay2 + ratio*(by2-ay2)
		};
	}

	public static float lerp(float a, float b, float c) {
		return a + c*(b-a);
	}

}
